#include "video_player/video_player_node.hpp"

// ROS 相关头文件
#include "rclcpp/logging.hpp"
#include "rclcpp/rclcpp.hpp"
#include "camera_info_manager/camera_info_manager.hpp"
#include "ament_index_cpp/get_package_share_directory.hpp" 

// 图像处理相关头文件
#include "cv_bridge/cv_bridge.hpp"
#include "image_transport/image_transport.hpp"
#include "sensor_msgs/msg/camera_info.hpp"
#include "sensor_msgs/msg/image.hpp"

// C++ 系统头文件
#include <string>
#include <thread>
#include <opencv2/opencv.hpp>

namespace video_player {

// -------------------------- 构造/析构函数实现 --------------------------
VideoPlayerNode::VideoPlayerNode(const rclcpp::NodeOptions &options) 
  : Node("video_player", options)
{
  RCLCPP_INFO(this->get_logger(), "Starting VideoPlayerNode...");

  // 1. 初始化视频捕获
  if (!initVideoCapture()) {
    RCLCPP_FATAL(this->get_logger(), "Video initialization failed! Node will exit.");
    rclcpp::shutdown();
    return;
  }

  // 2. 创建图像发布者
  createImagePublisher();

  // 3. 加载相机内参
  loadCameraInfo();

  // 4. 启动图像发布线程
  startPublishThread();

  RCLCPP_INFO(this->get_logger(), "VideoPlayerNode initialized successfully!");
}

VideoPlayerNode::~VideoPlayerNode()
{
  // 等待发布线程退出
  if (publish_thread_.joinable()) {
    publish_thread_.join();
    RCLCPP_INFO(this->get_logger(), "Publish thread joined successfully.");
  }

  RCLCPP_INFO(this->get_logger(), "VideoPlayerNode destroyed.");
}

// -------------------------- 核心初始化函数实现 --------------------------
bool VideoPlayerNode::initVideoCapture()
{
  // 设置视频文件路径（固定路径或可通过参数修改）

  std::string package_path = ament_index_cpp::get_package_share_directory("video_player");
  std::string video_path =package_path + "/config/demo.avi";

  // 尝试打开视频文件
  if (!cap_.open(video_path)) {
    RCLCPP_ERROR(this->get_logger(), "Failed to open video file: %s", video_path.c_str());
    return false;
  }

  // 获取视频基本信息
  frame_width_ = static_cast<int>(cap_.get(cv::CAP_PROP_FRAME_WIDTH));
  frame_height_ = static_cast<int>(cap_.get(cv::CAP_PROP_FRAME_HEIGHT));
  fps_ = cap_.get(cv::CAP_PROP_FPS);
  
  RCLCPP_INFO(this->get_logger(), "Video loaded successfully. Resolution: %dx%d, FPS: %.1f",
             frame_width_, frame_height_, fps_);
  
  return true;
}

void VideoPlayerNode::createImagePublisher()
{
  // 使用传感器数据QoS配置
  auto qos_profile = rmw_qos_profile_sensor_data;

  // 创建图像+内参联合发布者
  camera_pub_ = image_transport::create_camera_publisher(this, "image_raw", qos_profile);

  // 预分配图像数据内存
  size_t max_data_size = frame_width_ * frame_height_ * 3;
  image_msg_.data.reserve(max_data_size);

  // 初始化图像消息固定字段
  image_msg_.encoding = "bgr8";  // OpenCV默认是BGR格式
  image_msg_.header.frame_id = "camera_optical_frame";
}

void VideoPlayerNode::loadCameraInfo()
{
  // 相机名称和内参路径
  std::string camera_name = "video_camera";
  std::string camera_info_url = "package://video_player/config/camera_info.yaml";

  // 初始化内参管理器
  camera_info_manager_ = std::make_unique<camera_info_manager::CameraInfoManager>(this, camera_name);

  // 加载内参
  if (camera_info_manager_->validateURL(camera_info_url)) {
    if (camera_info_manager_->loadCameraInfo(camera_info_url)) {
      camera_info_msg_ = camera_info_manager_->getCameraInfo();
      RCLCPP_INFO(this->get_logger(), "Loaded camera info from URL: %s", camera_info_url.c_str());
    } else {
      RCLCPP_WARN(this->get_logger(), "Valid URL but failed to load camera info: %s", camera_info_url.c_str());
    }
  } else {
    RCLCPP_WARN(this->get_logger(), "Invalid camera info URL: %s", camera_info_url.c_str());
    // 使用默认空内参
    camera_info_msg_ = sensor_msgs::msg::CameraInfo();
  }

  // 初始化内参消息的frame_id
  camera_info_msg_.header.frame_id = image_msg_.header.frame_id;
  // 设置内参分辨率与视频一致
  camera_info_msg_.width = frame_width_;
  camera_info_msg_.height = frame_height_;
}

void VideoPlayerNode::startPublishThread()
{
  // 启动图像发布线程
  publish_thread_ = std::thread{[this]() -> void {
    RCLCPP_INFO(this->get_logger(), "Publish thread started. Publishing images...");

    // 计算帧间隔时间(毫秒)
    int frame_interval_ms = static_cast<int>(1000.0 / fps_);

    while (rclcpp::ok()) {
      // 开始处理一帧图像
      auto process_start = std::chrono::steady_clock::now();
      cv::Mat frame;
      try {
        // 读取一帧图像
        if (!cap_.read(frame)) {
          // 视频播放完毕，重新开始
          RCLCPP_INFO(this->get_logger(), "Video playback completed. Restarting...");
          cap_.set(cv::CAP_PROP_POS_FRAMES, 0);
          if (!cap_.read(frame)) {
            throw std::runtime_error("Failed to restart video playback");
          }
        }
        // 转换OpenCV图像为ROS消息
        cv_bridge::CvImage cv_img;
        cv_img.image = frame;
        cv_img.encoding = "bgr8";
        auto img_msg = cv_img.toImageMsg();

        // 更新图像消息信息
        auto current_time = this->now();
        img_msg->header.stamp = current_time;
        img_msg->header.frame_id = image_msg_.header.frame_id;
        img_msg->height = frame_height_;
        img_msg->width = frame_width_;
        img_msg->step = frame_width_ * 3;

        // 更新相机内参时间戳
        camera_info_msg_.header.stamp = current_time;

        // 发布图像和内参消息
        camera_pub_.publish(*img_msg, camera_info_msg_);

        // 计算实际处理耗时（毫秒）
        auto process_end = std::chrono::steady_clock::now();
        int process_ms = std::chrono::duration_cast<std::chrono::milliseconds>(process_end - process_start).count();

        // 动态计算需要休眠的时间（确保总耗时≈frame_interval_ms）
        int sleep_ms = frame_interval_ms - process_ms;
        if (sleep_ms > 0){
        std::this_thread::sleep_for(std::chrono::milliseconds(5));
        }
        //RCLCPP_INFO(this->get_logger(), "Published frame. Process time: %d ms, Sleep time: %d ms", process_ms, sleep_ms);
      }
      catch (const std::exception& e) {
        RCLCPP_ERROR(this->get_logger(), "Error processing frame: %s. Attempting recovery...", e.what());
        // 尝试重新初始化视频
        if (!initVideoCapture()) {
          RCLCPP_FATAL(this->get_logger(), "Failed to recover video playback. Exiting...");
          rclcpp::shutdown();
          return;
        }
      }
    }
  }};
}

}  // namespace video_player

// -------------------------- 注册 ROS 组件 --------------------------
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(video_player::VideoPlayerNode)